// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "WorldMath.h"

#include "nlohmann/json.hpp"
using json = nlohmann::json;

/**
 * Helper to convert between projected and geodetic coordinates.
 */
class FWorldEntityAxis
{

public:

	/**
	 * Example:
	 *
	 *              | Road Axis | Parameter Axis
	 *   OpenDRIVE: |   Proj    |    Inherit
	 *   WorldDB:   | Geodetic  |     Proj
	 *
	 *  Proj in OpenDRIVE is loaded from the proj.4 string in file header.
	 *  Proj in WorldDB params is tmerc, origin at road start point.
	 *  Inherit means no conversion. Use the numbers as-is.
	 */
	enum class Type
	{
		Unknown,
		Inherit,
		Proj,
		Geodetic
	};

	Type		SourceType;
	glm::dvec4	FalseOrigin;

public:

	glm::dvec3 ToProj(const glm::dvec3& source) const
	{
		if (SourceType == Type::Geodetic)
		{
			return GeodeticToProj(source);
		}
		else
		{
			return source;
		}
	}

	glm::dvec3 ToGeodetic(const glm::dvec3& source) const
	{
		if (SourceType == Type::Proj)
		{
			return ProjToGeodetic(source);
		}
		else
		{
			return source;
		}
	}

	glm::dvec3 ToProj(const glm::dvec3& source, const FWorldEntityAxis& targetAxis) const
	{
		if (SourceType == Type::Inherit)
		{
			// Inherits the projection from target axis. use the numbers as-is.
			return source;
		}
		else
		{
			// Convert back to geodetic and re-project into target axis.
			return targetAxis.ToProj(ToGeodetic(source));
		}
	}

	void ToProjPoly(glm::dvec2& a, glm::dvec2& b, glm::dvec2& c, glm::dvec2& d, const FWorldEntityAxis& targetAxis) const
	{
		if (SourceType == Type::Inherit)
		{
			// Inherits the projection from target axis. use the numbers as-is.
			return;
		}
		else
		{
			// To catmull control points in this axis. This assume t in range [0, 1].
			glm::dvec2 p0 = a - b + c + d;
			glm::dvec2 p1 = a;
			glm::dvec2 p2 = a + b + c + d;
			glm::dvec2 p3 = a + 2.0 * b + 4.0 * c + 6.0 * d;

			// Convert back to geodetic and re-project into target axis.
			glm::dvec3 org = ToProj(glm::dvec3(0.0), targetAxis);
			glm::dvec3 pp0 = ToProj(glm::dvec3(p0, 0.0), targetAxis) - org;
			glm::dvec3 pp1 = ToProj(glm::dvec3(p1, 0.0), targetAxis) - org;
			glm::dvec3 pp2 = ToProj(glm::dvec3(p2, 0.0), targetAxis) - org;
			glm::dvec3 pp3 = ToProj(glm::dvec3(p3, 0.0), targetAxis) - org;

			// Back to cubic poly parameters.
			glm::dvec3 pa = 0.5 * (2.0 * pp1);
			glm::dvec3 pb = 0.5 * (-pp0 + pp2);
			glm::dvec3 pc = 0.5 * (2.0 * pp0 - 5.0 * pp1 + 4.0 * pp2 - pp3);
			glm::dvec3 pd = 0.5 * (-pp0 + 3.0 * pp1 - 3.0 * pp2 + pp3);
			a = glm::dvec2(pa.x, pa.y);
			b = glm::dvec2(pb.x, pb.y);
			c = glm::dvec2(pc.x, pc.y);
			d = glm::dvec2(pd.x, pd.y);
		}
	}

	glm::dvec3 GeodeticToProj(const glm::dvec3& latlong) const;
	glm::dvec3 ProjToGeodetic(const glm::dvec3& projected) const;

public:

	static FWorldEntityAxis FromOpenDRIVE(const std::string& proj);
	static FWorldEntityAxis FromDatabase(const std::string& proj);
	static FWorldEntityAxis Unknown();
	static FWorldEntityAxis Inherit();

private:

	class Proj; // Proj.4
	std::shared_ptr<Proj>	_proj;

};
